COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	PROGRAM TO DRIVE A SINGLE JOINT ON THE ARMS USING THE PDP11 'AL SERVO
C00004 00003	⊃ PROCEDURES FOR CONVERTING TO AND FROM PDP11/45 NUMBER REPRESENTATION
C00006 00004	PROCEDURE DRIVE(INTEGER ARM,JOINT REAL ANGLE INTEGER DURA)
C00011 00005
C00023 ENDMK
C⊗;
COMMENT PROGRAM TO DRIVE A SINGLE JOINT ON THE ARMS USING THE PDP11 'AL SERVO;

BEGIN "DIAGY"

REQUIRE "TLKEF2.FAI" LOAD_MODULE;

DEFINE ⊃="COMMENT ",CRLF="('15&'12)",FUNCT="1",HEAD="2";
INTEGER ARM,DURA,DUM,JOINT,I,RUN,MASTER,K,J,ANG1,ANG2;
INTEGER LL,UL,POG,MODULUS,BP,DD,III;
INTEGER ARRAY DISPLY[1:'3000];
INTEGER ARRAY DATA[1:3601];
INTEGER ARRAY VEL[1:101],ANG[1:101],MOTOR[1:101];
INTEGER ARRAY TH[1:600],THD[1:600],TORQUE[1:600],STIME[1:600];
STRING WARM,ANS,LINED,GANS,COM1,COM2,COM3;
STRING DISCM;
STRING DANS,RELD;
REAL ANGLE;
LABEL AMPLP,RUNELF,LPEND,BOTTOM;
LABEL ARMLP,MDONE;

EXTERNAL INTEGER PROCEDURE TLKEF2(INTEGER ARM,JOINT,DURA,ANGLE1,ANGLE2,
       MASTER);
⊃ PROCEDURES FOR CONVERTING TO AND FROM PDP11/45 NUMBER REPRESENTATION;

REAL PROCEDURE GETFLT(INTEGER NUM1,NUM2);
	BEGIN
		INTEGER LBYTE;
		REAL Z;
		LABEL DNE;
		LBYTE←'142700000002;
		START_CODE
			MOVE   1,NUM1;
			LSH    1,16;
			MOVE   2,NUM2;
			ANDI   2,'177777;
			IOR    1,2;   
			SETZ   2,;
			DPB    1,LBYTE;
			LSH    1,-23;
			LSHC   1,-9;    
			JUMPE  2,DNE;
			TLO    2,'400;
			JUMPGE 2,DNE;
			TLZ    2,'400000;
			MOVN   2,2;
		  DNE:	MOVEM  2,Z;
             	END;
		RETURN(Z);
	END;


PROCEDURE CONELF(REFERENCE INTEGER NUM1,NUM2;REAL T);
	BEGIN
		INTEGER BYTE;
		LABEL ST1,ST2,OVER;
		BYTE←'013200000002;
		START_CODE
		   	MOVE   2,T;
			JUMPGE 2,ST1;
			MOVN   2,2;
 			TLO    2,'400000;
		ST1:	JFCL   2,ST2;
		ST2:	ADDI   2,4;
			JFCL   2,OVER;
     		    	DPB    2,BYTE;
			SETZ   1,;
			LSHC   1,16;
			MOVEM  1,NUM1;
			SETZ   1,;
			LSHC   1,16;
			MOVEM  1,NUM2;
		END;
		RETURN;
	OVER:	OUTSTR("ERROR-ROUNDING OVERFLOW"&CRLF);
		RETURN; 
	END; 
PROCEDURE DRIVE(INTEGER ARM,JOINT; REAL ANGLE; INTEGER DURA);
BEGIN "DRIVE"
⊃ ARM← 1 FOR BLUE AND 0 FOR YELLOW;

⊃ START OF RUN TIME CODE;

	MASTER←'13131;

⊃ SEND COMMAND TO THE ELF. IF RUN IS NOT ZERO, SOMETHINGS WRONG WITH THE
  ELF;

	CONELF(ANG1,ANG2,ANGLE);
  	RUN←TLKEF2(ARM,JOINT,DURA,ANG1,ANG2,MASTER,DATA);
	IF RUN=1 THEN GOTO AMPLP
   	ELSE IF RUN≠0 THEN OUTSTR("ARM STATUS = "&CVOS(-RUN)&CRLF);

END "DRIVE";